#include <iostream>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include "ros_web_bridge/draco_pcl_converter.hpp"

class DracoTCPReceiver {
private:
    int server_fd, client_fd;
    struct sockaddr_in address;
    
public:
    bool startServer(int port) {
        server_fd = socket(AF_INET, SOCK_STREAM, 0);
        if (server_fd == 0) {
            std::cerr << "Socket creation failed" << std::endl;
            return false;
        }
        
        int opt = 1;
        if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, 
                       &opt, sizeof(opt))) {
            std::cerr << "Setsockopt failed" << std::endl;
            return false;
        }
        
        address.sin_family = AF_INET;
        address.sin_addr.s_addr = INADDR_ANY;
        address.sin_port = htons(port);
        
        if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0) {
            std::cerr << "Bind failed" << std::endl;
            return false;
        }
        
        if (listen(server_fd, 3) < 0) {
            std::cerr << "Listen failed" << std::endl;
            return false;
        }
        
        std::cout << "Server listening on port " << port << std::endl;
        return true;
    }
    
    pcl::PointCloud<pcl::PointXYZI>::Ptr receiveCompressedPointCloud() {
        int addrlen = sizeof(address);
        client_fd = accept(server_fd, (struct sockaddr*)&address, (socklen_t*)&addrlen);
        if (client_fd < 0) {
            std::cerr << "Accept failed" << std::endl;
            return nullptr;
        }
        
        std::cout << "Client connected" << std::endl;
        
        // 接收数据大小
        uint32_t data_size;
        if (recv(client_fd, &data_size, sizeof(data_size), 0) <= 0) {
            std::cerr << "Error receiving data size" << std::endl;
            return nullptr;
        }
        
        std::cout << "Expecting " << data_size << " bytes" << std::endl;
        
        // 接收压缩数据
        std::vector<uint8_t> compressed_data(data_size);
        size_t total_received = 0;
        while (total_received < data_size) {
            ssize_t received = recv(client_fd, compressed_data.data() + total_received,
                                   data_size - total_received, 0);
            if (received <= 0) {
                std::cerr << "Error receiving data" << std::endl;
                return nullptr;
            }
            total_received += received;
        }
        
        std::cout << "Received " << total_received << " bytes, decompressing..." << std::endl;
        
        // 解压缩
        auto decompressed_cloud = DracoPCLConverter::DecompressPointCloud(compressed_data);
        if (decompressed_cloud) {
            std::cout << "Decompressed point cloud with " << decompressed_cloud->size() 
                      << " points" << std::endl;
        }
        
        close(client_fd);
        return decompressed_cloud;
    }
    
    ~DracoTCPReceiver() {
        if (server_fd >= 0) {
            close(server_fd);
        }
    }
};

int main() {
    DracoTCPReceiver receiver;
    if (receiver.startServer(9998)) {
        auto received_cloud = receiver.receiveCompressedPointCloud();
        if (received_cloud) {
            // 保存接收到的点云
            pcl::io::savePCDFile("received_terrain.pcd", *received_cloud);
            std::cout << "Point cloud saved to received_terrain.pcd" << std::endl;
        }
    }
    
    return 0;
}